#include "stdafx.h"
#include "..\include\matrix3.h"
#include "..\include\AxisAlignedBox.h"
#include "..\include\Plane.h"


namespace Common {
	//-----------------------------------------------------------------------
	Plane::Plane ()
	{
		normal = Vector3::ZERO;
		d = 0.0;
	}
	//-----------------------------------------------------------------------
	Plane::Plane (const Plane& rhs)
	{
		normal = rhs.normal;
		d = rhs.d;
	}
	//-----------------------------------------------------------------------
	Plane::Plane (const Vector3& rkNormal, Real fConstant)
	{
		normal = rkNormal;
		d = fConstant;
	}
	//---------------------------------------------------------------------
	Plane::Plane (Real a, Real b, Real c, Real _d)
		: normal(a, b, c), d(_d)
	{
	}
	//-----------------------------------------------------------------------
	Plane::Plane (const Vector3& rkNormal, const Vector3& rkPoint)
	{
		redefine(rkNormal, rkPoint);
	}
	//-----------------------------------------------------------------------
	Plane::Plane (const Vector3& rkPoint0, const Vector3& rkPoint1,
		const Vector3& rkPoint2)
	{
		redefine(rkPoint0, rkPoint1, rkPoint2);
	}
	//-----------------------------------------------------------------------
	Real Plane::getDistance (const Vector3& rkPoint) const
	{
		return normal.dotProduct(rkPoint) - d;
	}
	//-----------------------------------------------------------------------
	bool Plane::getDistance (const Plane& rPlane, Real& fDistance) const
	{
		//remember, portal normals face the opposite direction to boundary normals.
		const float normalTolerance = -0.999f;
		if ( this->normal.dotProduct( rPlane.normal ) < normalTolerance )
		{
			Common::Vector3 pts1( rPlane.normal * rPlane.d );
			Common::Vector3 pts2( this->normal * this->d );

			fDistance = ( pts1 - pts2 ).length();
			
			return true;
		}

		return false;
	}
	//-----------------------------------------------------------------------
	Plane::Side Plane::getSide (const Vector3& rkPoint) const
	{
		Real fDistance = getDistance(rkPoint);

		if ( fDistance < 0.0 )
			return Plane::NEGATIVE_SIDE;

		if ( fDistance > 0.0 )
			return Plane::POSITIVE_SIDE;

		return Plane::NO_SIDE;
	}


	//-----------------------------------------------------------------------
	Plane::Side Plane::getSide (const AxisAlignedBox& box) const
	{
		if (box.isNull()) 
			return NO_SIDE;
		if (box.isInfinite())
			return BOTH_SIDE;

        return getSide(box.getCenter(), box.getHalfSize());
	}
    //-----------------------------------------------------------------------
    Plane::Side Plane::getSide (const Vector3& centre, const Vector3& halfSize) const
    {
        // Calculate the distance between box centre and the plane
        Real dist = getDistance(centre);

        // Calculate the maximise allows absolute distance for
        // the distance between box centre and plane
        Real maxAbsDist = normal.absDotProduct(halfSize);

        if (dist < -maxAbsDist)
            return Plane::NEGATIVE_SIDE;

        if (dist > +maxAbsDist)
            return Plane::POSITIVE_SIDE;

        return Plane::BOTH_SIDE;
    }
	//-----------------------------------------------------------------------
	void Plane::redefine(const Vector3& rkPoint0, const Vector3& rkPoint1,
		const Vector3& rkPoint2)
	{
		Vector3 kEdge1 = rkPoint1 - rkPoint0;
		Vector3 kEdge2 = rkPoint2 - rkPoint0;
		normal = kEdge1.crossProduct(kEdge2);
		normal.normalise();
		d = normal.dotProduct(rkPoint0);
	}
	//-----------------------------------------------------------------------
	void Plane::redefine(const Vector3& rkNormal, const Vector3& rkPoint)
	{
		normal = rkNormal;
		d = rkNormal.dotProduct(rkPoint);
	}
	//-----------------------------------------------------------------------
	Vector3 Plane::projectVector(const Vector3& p) const
	{
		// We know plane normal is unit length, so use simple method
		Matrix3 xform;
		xform[0][0] = 1.0f - normal.x * normal.x;
		xform[0][1] = -normal.x * normal.y;
		xform[0][2] = -normal.x * normal.z;
		xform[1][0] = -normal.y * normal.x;
		xform[1][1] = 1.0f - normal.y * normal.y;
		xform[1][2] = -normal.y * normal.z;
		xform[2][0] = -normal.z * normal.x;
		xform[2][1] = -normal.z * normal.y;
		xform[2][2] = 1.0f - normal.z * normal.z;
		return xform * p;

	}
	//-----------------------------------------------------------------------
    Real Plane::normalise(void)
    {
        Real fLength = normal.length();

        // Will also work for zero-sized vectors, but will change nothing
        if (fLength > 1e-08f)
        {
            Real fInvLength = 1.0f / fLength;
            normal *= fInvLength;
            d *= fInvLength;
        }

        return fLength;
    }
	//-----------------------------------------------------------------------
	const int biMoreL[3]={ 1, 2, 0 };
	const int biLessL[3]={ 2, 0, 1 };

	/**
	*	This function returns the x and y directions for the parameterised
	*	basis of the given plane.
	*/
	void Plane::_basis( Vector3& xdir, Vector3& ydir ) const
	{
		// find a suitable x basis vector  (find biggest dimension of normal.
		//  set other dimension A to zero. set other dimension B to one.
		//  then calc what biggest dimension should be to put us on the plane.
		//  we choose the biggest because we have to divide by it)
		float fabses[3] =
		{ float(fabs(normal.x)), float(fabs(normal.y)), float(fabs(normal.z)) };
		int bi = fabses[0] > fabses[1] ?
			( fabses[0] > fabses[2] ? 0 : 2 ) : (fabses[1] > fabses[2] ? 1 : 2);

		int biMore = biMoreL[ bi ];
		int biLess = biLessL[ bi ];

		xdir[ biLess ] = 0;
		xdir[ biMore ] = 1;
		xdir[ bi     ] = -normal[ biMore ] / normal[ bi ];

		// find a suitable y basis vector  (cross product of existing ones)
		ydir = normal.crossProduct( xdir );

		xdir.normalise();
		ydir.normalise();
	}
	//-----------------------------------------------------------------------
	/**
	*	This function finds the point defined by the 'point' param on the
	*	given plane.
	*/
	Vector3 Plane::unprojectTo3DCoord( const Vector2& param ) const
	{
		// get the basis to use
		Vector3 xdir, ydir;
		this->_basis( xdir, ydir );

		float dOverNormLen = d / normal.squaredLength();

		// and get the vector. the 'origin' of the plane is at
		//  normal * d / normal.lengthSquared
		return dOverNormLen * normal + param.x * xdir + param.y * ydir;
	}

	//-----------------------------------------------------------------------

	/**
	*	This function finds the param for the given point in the plane's basis
	*/
	Vector2 Plane::projectTo2DCoord( const Vector3& rPts ) const
	{
		// get the basis to use
		Vector3 xdir, ydir;
		this->_basis( xdir, ydir );

		// take off the origin of the plane
		Vector3 offset = rPts - ( d / normal.squaredLength() ) * normal;

		// and project it onto our (unit length) basis vectors
		return Vector2(
			xdir.dotProduct( offset ),
			ydir.dotProduct( offset ) );
	}

	//-----------------------------------------------------------------------

	std::ostream& operator<< (std::ostream& o, const Plane& p)
	{
		o << "Plane(normal=" << p.normal << ", d=" << p.d << ")";
		return o;
	}
} // namespace Ogre
